// Kevin Kemper

#include <fca_controllers/controller.h>
#include <rtai.h>

// Torque constant (Nm/Arms):
#define TC		-0.18

// Gear ratio:
#define GR		17.25


// spring 1

#define K		125.000000
#define B		1.000000
#define I		0.049118 
#define OFFSET	-0.007351


// spring 2
/*
#define K		550.000000
#define B		1.200000
#define I		0.079118
#define OFFSET	-0.017709
*/

// spring 3
/*
#define K		1400.000000
#define B		1.500000
#define I		0.090118
#define OFFSET	-0.021499
*/

#define A11		0
#define A12		1
#define A21		(-K/I)
#define A22		(-B/I)

#define B1		0
#define B2		(1/I)

#define C11		1
#define C12		0
#define C21		0
#define C22		1


#define INVCB1	(0.0)
//#define INVCB2	(1/B2)
#define INVCB2	(I)


#define T2A		((1.0/TC)*(1.0/GR))

//	A = [0, 1;
//		0, 0];
//	B = [0; G/I];
//	C = [1, 0;
//		0, 1];

/*	INPUT members:

	float			motor_angle;
	float			leg_angle;

	float			motor_velocity;
	float			leg_velocity;

	unsigned char	motor_current;

	unsigned char	command;
*/


//	e = yd(t) - C * x;
//	u = - K * x + (C * B) \ ( ydd(t) - C*(A - B*K)*x + Ke * e );
//	xdot = A * x + B *u;

//	yd = @(t) [ sin(t); cos(t) ];
//	ydd = @(t) -sin(t);


extern void initialize_torque_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.0;
	output->time = 0.0;
	TORQUE_SINE_CONTROLLER_STATE(state)->time = 0.0;
}


extern void update_torque_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, ControllerData *data) {

	float spinrg_k		= K;
	float spring_pos	= input->motor_angle	- input->leg_angle + OFFSET;
	float spring_vel	= input->motor_velocity	- input->leg_velocity;
	float spring_force	= spinrg_k * spring_pos;
	
	float t;
	
	float a		= TORQUE_SINE_CONTROLLER_DATA(data)->trq_amp;
	float w		= TORQUE_SINE_CONTROLLER_DATA(data)->trq_freq;
	float off	= TORQUE_SINE_CONTROLLER_DATA(data)->trq_off;

	float k1	= -TORQUE_SINE_CONTROLLER_DATA(data)->trq_k1;
	float k2	= -TORQUE_SINE_CONTROLLER_DATA(data)->trq_k2;
	
	float ke1	= TORQUE_SINE_CONTROLLER_DATA(data)->trq_ke1;
	float ke2	= TORQUE_SINE_CONTROLLER_DATA(data)->trq_ke2;

	float err1,err2;
	float tmp1,tmp2;
	float bk11,bk12,bk21,bk22;
	float abk11,abk12,abk21,abk22;
	float cabk11,cabk12,cabk21,cabk22;

	float yd1, yd2;
	float ydd;
	
	float c1,c2;
	
	float u;
	
	float x1 = spring_pos;
	float x2 = spring_vel;
	
	
	TORQUE_SINE_CONTROLLER_STATE(state)->time += 0.001;
	output->time = TORQUE_SINE_CONTROLLER_STATE(state)->time;
		
	t	= TORQUE_SINE_CONTROLLER_STATE(state)->time;


	yd1	= (off+a*sin(w*t))/K;
	yd2	= (a*w*cos(w*t))/K;
	ydd	= (a*w*w*-sin(w*t))/K;

	err1 = yd1 - (C11*x1 + C12*x2);
	err2 = yd2 - (C21*x1 + C22*x2);

	bk11 = B1*k1;
	bk12 = B1*k2;
	bk21 = B2*k1;
	bk22 = B2*k2;
	
	abk11 = A11-bk11;
	abk12 = A12-bk12;
	abk21 = A21-bk21;
	abk22 = A22-bk22;
	
	cabk11 = C11*abk11+C12*abk21;
	cabk12 = C11*abk12+C12*abk22;
	cabk21 = C21*abk11+C22*abk21;
	cabk22 = C21*abk12+C22*abk22;
	
	tmp1 = ydd - (cabk11*x1+cabk12*x2) + ke1*err1 + ke2*err2;
	tmp2 = ydd - (cabk21*x1+cabk22*x2) + ke1*err1 + ke2*err2;

	c1 = (I*w*w*w*B)/(w*w*B*B+K*K);
	c2 = (w*w*B*B-I*w*w*K+K*K)/(w*w*B*B+K*K);

	u = -(k1*x1+k2*x2) + INVCB1*tmp1 + INVCB2*tmp2; //+ a*c1*cos(w*t) + a*c2*sin(w*t);
	
//	u = -(k1*x1+k2*x2) + INVCB1*ydd + INVCB2*ydd; //+ a*c1*cos(w*t) + a*c2*sin(w*t);
	
	rt_printk( "tmp1 %d   tmp2 %d\n", (int)(tmp1), (int)(tmp2));

	if (u > TORQUE_SINE_CONTROLLER_DATA(data)->trq_lim) {
		output->motor_torque =  T2A*TORQUE_SINE_CONTROLLER_DATA(data)->trq_lim;
	}
	else if (u < -TORQUE_SINE_CONTROLLER_DATA(data)->trq_lim) {
		output->motor_torque = -T2A*TORQUE_SINE_CONTROLLER_DATA(data)->trq_lim;
	}
	else {
		output->motor_torque = T2A*u;
	}
	

}


extern void takedown_torque_sine_controller(ControllerInput *input, ControllerOutput *output, ControllerState *state, 
	ControllerData *data)
{
	output->motor_torque = 0.0;
	output->time = 0.0;
	TORQUE_SINE_CONTROLLER_STATE(state)->time = 0.0;
}
